#include "Level.h"

#include "Game.h"

#define TILE_SIZE 512

void CLevel::load(String sName)
{
    
    m_Background.load(LEVELS_PATH + sName + "_bg" + TEXTURE_EXT, TILE_SIZE, TILE_SIZE);
    m_Foreground.load(LEVELS_PATH + sName + "_fg" + TEXTURE_EXT, TILE_SIZE, TILE_SIZE);
    
    Texture maskTexture;
    
    unsigned char* cpRawMaskData = CGame::getInstance()->getTextureHandler()->loadRawImageData((LEVELS_PATH + sName + "_mask" + TEXTURE_EXT).c_str(), maskTexture);
    
    //Create a new RGB data array
    m_iMaskData = new long[maskTexture.iWidth * maskTexture.iHeight];
    
    //TODO: Check that different bitdepths are allowed for in here
    
    //TODO: Replace this copy code with memcpy
    //Iterate through the bytes in the array
    for (int i=0, j=0;i<maskTexture.iWidth * maskTexture.iHeight * maskTexture.iBitDepth / 8; i+=3, ++j) {
        
        m_iMaskData[j] = cpRawMaskData[i]*256*256 + cpRawMaskData[i+1]*256 + cpRawMaskData[i+2];
        
        //Skip the alpha byte if it is present
        if (maskTexture.iBitDepth == 32)
            ++i;
        
    }
    
    //We no longer need the raw image data
    delete [] cpRawMaskData;
    
    m_iMaskWidth = maskTexture.iWidth;
    m_iMaskHeight = maskTexture.iHeight;
    
}

void CLevel::drawBackground()
{
    
    m_Background.render();
    
}

void CLevel::drawForeground()
{
    
    m_Foreground.render();
    
}

void CLevel::collideWith(CEntity* pEntity)
{
    
    //Adjust pos for center origin instead of corner origin as in textures
    Vector vPos = pEntity->getPos() + Vector(m_iMaskWidth / 2.f, m_iMaskHeight / 2.f);
    Scalar fRadius = pEntity->getCollisionRadius();
    
    if (countCollidingPixels(vPos, fRadius)) {
        
        int iDX = countCollidingPixels(vPos - Vector(1, 0), fRadius) - countCollidingPixels(vPos + Vector(1, 0), fRadius);
        int iDY = countCollidingPixels(vPos - Vector(0, 1), fRadius) - countCollidingPixels(vPos + Vector(0, 1), fRadius);
        
        Vector vNormal = Vector(iDX, iDY);
        //std::cout << vNormal.Length() << std::endl;
        //TODO: Work out how to find depth of overlap
        //pEntity->getPos() -= 0.01 * vNormal;
        
        Scalar fNormalLength = vNormal.getLength();
        
        vNormal /= fNormalLength;
        
        Vector vContactPos = pEntity->getPos() - vNormal * fRadius;
        
        pEntity->getPhysics()->setVel((pEntity->getPos() - pEntity->getPhysics()->getOldPos()) * 0.5f);
        
        Vector vShift = vNormal * fNormalLength / 10.f;//vNormal * (pEntity->getPhysics().getVel() % vNormal);
        //Vector vShift = vNormal + vNormal * (pEntity->getPhysics().getVel() % vNormal);
        //Vector vShift = vNormal * 2.f + pEntity->getPos() - pEntity->getPhysics().getOldPos();
        
        pEntity->getPos() += vShift;
        pEntity->getPhysics()->getOldPos() += vShift;
        
        //1.0f means all energy absorbed, 2.0f means perfect bounce
        Scalar fBounce = 3.0f;
        
        //Apply normal reaction force
        Scalar fNormalForce = fBounce * 0.5f * (pEntity->getPhysics()->getVel() % -vNormal);
        pEntity->applyForce(vNormal * fNormalForce, vContactPos);
        
        //Apply friction
        
        Scalar fSpinVel = pEntity->getPhysics()->getAngVel() * fRadius;
        
        Scalar fFrictionCoef = (getMaskValue(vContactPos - vNormal * 2.f) & 0x0000FF) / 255.f;//0.5f;
        //std::cout << fFrictionCoef << std::endl;
        //std::cout << vNormal[0] << ", " << vNormal[1] << std::endl;
        
        fFrictionCoef = 1.0;
        //+2.f * vNormal*fRadius
        pEntity->applyForce(fFrictionCoef * -vNormal.getNormal() * (-0.30f * fSpinVel + (pEntity->getPhysics()->getVel() % vNormal.getNormal())), vContactPos);
        
        pEntity->getPhysics()->setAngVel(pEntity->getPhysics()->getAngVel() * 1.0f);
        
        vContactPos += vNormal * 40.f;
        
        glBindTexture(GL_TEXTURE_2D, 0);
        
        /*glBegin(GL_POINTS);
        glColor4f(1, 1, 1, 1);
        glVertex3dv(vContactPos.getArray());
        glEnd();*/
        //pEntity->setIsOnGround(true);
        
    }
    
}

/*void CLevel::collideWith(CPlayer* player)
{
    
    
    
}*/

int CLevel::countCollidingPixels(Vector vPos, Scalar fRadius)
{
    
    int iCollidedPixelCount = 0;
    
    int iStartX = (int)(vPos[0]-fRadius);
    int iEndX = (int)(vPos[0]+fRadius);
    int iStartY = (int)(vPos[1]-fRadius);
    int iEndY = (int)(vPos[1]+fRadius);
    
    //Clip the region at the edges of the map mask
    if (iStartX < 0)
        iStartX = 0;
    else if (iEndX > m_iMaskWidth)
        iEndX = m_iMaskWidth;
    if (iStartY < 0)
        iStartY = 0;
    else if (iEndY > m_iMaskHeight)
        iEndY = m_iMaskHeight;
    
    Scalar fRadius2 = fRadius*fRadius;
    
    long iColor;
    
    for (int x = iStartX; x < iEndX; ++x) {
        for (int y = iStartY; y < iEndY; ++y) {
            
            if ((x-vPos[0])*(x-vPos[0])+(y-vPos[1])*(y-vPos[1]) < fRadius2) {
                
                iColor = m_iMaskData[y * m_iMaskWidth + x];
                
                if (iColor != 0) {
                    
                    ++iCollidedPixelCount;
                    
                }
                
            }
            
        }
    }
    
    return iCollidedPixelCount;
    
}

long CLevel::getMaskValue(Vector vPos)
{
    
    //Adjust for centered image
    vPos += Vector(m_iMaskWidth / 2.f, m_iMaskHeight / 2.f);
    
    //Clamp the position at the edges of the map mask
    if (vPos[0] < 0)
        vPos[0] = 0;
    else if (vPos[0] > m_iMaskWidth)
        vPos[0] = m_iMaskWidth;
    if (vPos[1] < 0)
        vPos[1] = 0;
    else if (vPos[1] > m_iMaskHeight)
        vPos[1] = m_iMaskHeight;
    //std::cout << "Sample pos = " << vPos[0] << ", " << vPos[1] << std::endl;
    return m_iMaskData[(int)(vPos[1] * m_iMaskWidth + vPos[0])];
        
}
